#include <stdarg.h>
#include "print.h"

#define PRINT_BUF_SIZE  (256)

uint32_t get_sys_time_ms(void)
{
    uint32_t time_ms = 0;

    return time_ms;
}

int _my_printf(char * pFile, uint32_t Line, char *pFormat, ...)
{
    uint8_t printf_buf[PRINT_BUF_SIZE];

    uint8_t *p = printf_buf;
    uint32_t time = get_sys_time_ms();
    uint32_t len;
    int32_t res = 0;

    va_list args;

    len = PRINT_BUF_SIZE;

    res = snprintf ((char *)p, len, "<%u> %s->%u: ", time, pFile, Line);
    if ((res < 0) || (res >= len))
    {
        return 0;
    }

    p += res;
    len -= res;

    va_start (args, pFormat);
    res = vsnprintf ((char *)p, len, (char *)pFormat, args);
    va_end (args);

    if ((res < 0) || ((res + 2) > len))
    {
        return 0;
    }

    p += res;
    len -= res;

    *(p++) = '\n';
    *(p++) = '\0';

#ifdef USING_ST_LINK
    uint16_t print_len = p - &printf_buf[0];
    for(uint16_t i = 0; i < print_len; i++) {
        ITM_SendChar(printf_buf[i]);
    }
#endif
#ifdef USING_JLINK
    SEGGER_RTT_Write(0, printf_buf, p - &printf_buf[0]);
#endif

    printf("%s", printf_buf);
    return 1;
}

#ifdef USING_ST_LINK
int fputc(int ch, FILE *f)
{
    ITM_SendChar(ch);
    return ch;
}
#endif
#ifdef USING_JLINK
int fputc(int ch, FILE *f)
{
    SEGGER_RTT_PutChar(0, ch);
    return ch;
}
#endif

